import HWT9073
import time
import os
import sys
import psutil
import csv
import numpy as np


Datafolder = 'Data/IMUData/'
formatted_time = time.strftime('%Y%m%d%H%M%S',time.localtime())
IMU_data_file = Datafolder + f'IMU_data_{formatted_time}.csv'

# 进程查询，防止多运行
def checkProcess(processName):
    # processName = os.path.basename(__file__)
    ps1=psutil.pids()
    curps=os.getpid()
    for pid in ps1:
        try:
            cmdline = psutil.Process(pid).cmdline()
        except psutil.NoSuchProcess:
            continue
        # print(cmdline)
        if processName in cmdline and pid != curps and pid != (curps - 1):
            print(f"{processName}:, This is the second process pid = {str(pid)}, so exit!")
            sys.exit(1)
    print(f"now runing {processName},current pid = "+str(curps))
    return

def updateData(DeviceModel):
    timestamp = time.time()
    IMU_DATA = np.array([timestamp, DeviceModel.get("AngX"), DeviceModel.get("AngY"), DeviceModel.get("AngZ"), DeviceModel.get("AccX"), DeviceModel.get("AccY"), DeviceModel.get("AccZ")])
    # print(IMU_DATA)
    with open(IMU_data_file, "a") as f:
        writer = csv.writer(f)
        writer.writerow(IMU_DATA)
    # print(DeviceModel.deviceName + " : " + DeviceModel.deviceData)


if __name__ == "__main__":
    _fileName = sys.argv[1]
    checkProcess(_fileName)
    if not os.path.exists(Datafolder):
        os.makedirs(Datafolder)
    with open(IMU_data_file, "a") as f:
        writer = csv.writer(f)
        writer.writerow(["Time","AngX","AngY","AngZ","AccX","AccY","AccZ"])
    
    # 拿到设备模型 Get the device model
    device = HWT9073.DeviceModel("HWT9073 IMU", "/dev/ttyUSB0", 115200, 0x50, updateData)
    # 开启设备 Turn on the device
    device.openDevice()
    # 开启轮询 Enable loop reading
    device.startLoopRead()

    # 读取寄存器 从0x3a读取1个寄存器 Read Register Read 1 register from 0x3a
    # device.readReg(0x3a, 1)
    # 获得读取结果 Obtaining read results
    # device.get(str(0x3a))

    # 写入寄存器 向0x65写入50 即修改检测周期为50hz Write a register to 0x65 and write 50 to modify the detection cycle to 50Hz
    # device.writeReg(0x65, 50)